/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Collision detector
 */

// ROS
#include "ros/ros.h"
// Messages
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/Vector3.h"
#include "group6/convertCoordinates.h"
// Stdlibs
#include "math.h"
// Application
#include "coordinatorConverter.h"

#define USE_MATRIX

/*  Default constructor.
 *
 */
CoordinatorConverter::CoordinatorConverter()
{
    mServoAnglesSubscriber =            mNode.subscribe("ptu_servo_angles", 1000, &CoordinatorConverter::saveAngles, this);
    mServoAnglesSubscriber =            mNode.subscribe("saveRobotPosition", 1000, &CoordinatorConverter::saveRobotPosition, this);
    mConvertCoordinatesService =        mNode.advertiseService("convertCoordinates", &CoordinatorConverter::convertCoordinates, this);

    // memset(&mCurrentTransform, 0x00, sizeof(mCurrentTransform));
    memset(&mServoAngles, 0x00, sizeof(mServoAngles));
    memset(&mBasePosition, 0x00, sizeof(mBasePosition));
    memset(&mHeading, 0x00, sizeof(mHeading));
    mRealtime = true;
}

/*  Main.
 *
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "coordinatorConverter");
    CoordinatorConverter instance;

    ros::spin();

    return 0;
}

void print(const char* text, Pos position)
{
    if(text == NULL) return;

    ROS_INFO("%s - x: %.3f\ty: %.3f\tz: %.3f", text, position.x, position.y, position.z);
}

void CoordinatorConverter::saveRobotPosition(const std_msgs::Bool::ConstPtr& msg)
{
    if(msg->data == true)
    {
        requestPose();

        if(mRealtime == true)
        {
            ROS_INFO("> Convert changed to non-realtime");
        }
        mRealtime = false;
    }
    else
    {
        if(mRealtime == false)
        {
            ROS_INFO("> Convert changed to realtime");
        }
        mRealtime = true;
    }
}

bool CoordinatorConverter::requestPose()
{
    /// Get robot's position and heading
    try
    {
        mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));
   
        mListener.lookupTransform("/map", "/base_link",ros::Time(0), mCurrentTransform);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        return false;
        
    }
    ROS_INFO("own position received");      

    mHeading = limitAngle(tf::getYaw(mCurrentTransform.getRotation()));
    mBasePosition.x = mCurrentTransform.getOrigin().x();
    mBasePosition.y = mCurrentTransform.getOrigin().y();
    mBasePosition.z = mCurrentTransform.getOrigin().z();
    return true;
}


/*  
 *
 *  @param  scan  Message from LMS100
 *  @return void
 */
void CoordinatorConverter::saveAngles(const geometry_msgs::Vector3::ConstPtr& vector)
{
    mServoAngles.x = vector->x;
    mServoAngles.y = vector->y;
    mServoAngles.z = vector->z;
    ROS_INFO("Updated servo angles: elevation: %f, yaw: %f", mServoAngles.z, mServoAngles.y);
}


/*  
 *
 *  @param  req     Request
 *  @param  res     Respond
 *  @return bool    
 */
bool CoordinatorConverter::convertCoordinates(group6::convertCoordinates::Request &req, group6::convertCoordinates::Response &res)
{ 
    ROS_INFO("service called");
    Pos kinect; // Object's position in kinect's coordination
    kinect.x = req.point.x;
    kinect.y = req.point.y;
    kinect.z = req.point.z;

    //double elevation    = mServoAngles.z;
    double yaw          = mServoAngles.y;
    double elevation    = 55.0;
    Pos robot = convert(kinect, elevation, yaw); // Object's position in robot's coordination
    float distance_from_robot = sqrt((robot.x * robot.x) + (robot.y * robot.y));
    float angle_from_robot = atan2(robot.y, robot.x);
    ROS_INFO("converted");

    if(mRealtime)
    {
        requestPose();
    }

    /*
    mHeading = 0.0;
    mBasePosition.x = -0.01;
    mBasePosition.y = -0.01;
    mBasePosition.z = 0.0;
    */

    Pos base; // Object's position in world's coordination
    base.x = cos(mHeading + angle_from_robot) * distance_from_robot + mBasePosition.x;
    base.y = sin(mHeading + angle_from_robot) * distance_from_robot + mBasePosition.y;
    base.z = robot.z + mBasePosition.z;

    res.point.x = base.x;
    res.point.y = base.y;
    res.point.z = base.z;

    ROS_INFO("Convert %srealtime", mRealtime?"":"not ");
    ROS_INFO("Current position: x = %f, y = %f, z = %f, a = %f", mBasePosition.x, mBasePosition.y, mBasePosition.z, mHeading);
    ROS_INFO("Reported kinect angle: %.1f", elevation);
    print("Kinect's view", kinect);
    print("Robot's view ", robot);
    print("World's view ", base);
    ROS_INFO("---");
    return true;
}


Pos CoordinatorConverter::convert(Pos position, int kinectElevation, int kinectYaw)
{
    #if 0
    double kinectin_paikka_x = 0.0;
    double kinectin_paikka_y = 0.0;
    double kinectin_paikka_z = 0.0;
    double kinectin_varren_pituus = -0.1;
    double kinectin_etaisyys_kinect_basesta = 0.0;
    #else
    double kinectin_paikka_x = 0.07;
    double kinectin_paikka_y = 0.0;
    double kinectin_paikka_z = 0.33;
    double kinectin_varren_pituus = -0.065;         // Korkeussuunnassa, kinectin origon eätäisyys yläservon pyörimisakselista
    double kinectin_etaisyys_kinect_basesta = 0.02; // Pituussuunnassa, kinectin origin etäisyys alaservon pyörimisakselista
    #endif

    double elevationRadians = TO_RADIANS(90 - kinectElevation);
    double yawRadians = TO_RADIANS(90 - kinectYaw);
    Pos robotBase2;
#ifndef USE_MATRIX
/// Lens
    Pos lensBase;
    lensBase.x  = position.x;
    lensBase.y  = position.y + kinectin_varren_pituus;
    lensBase.z  = position.z;

/// Elevation servo
    Pos joint;

    joint.x = lensBase.x;
    joint.y = cos(elevationRadians) * lensBase.y 
            + sin(elevationRadians) * lensBase.z;
    joint.z = -sin(elevationRadians) * lensBase.y 
            + cos(elevationRadians) * lensBase.z 
            + kinectin_etaisyys_kinect_basesta;

/// Yaw servo
    Pos kinectBase;

    kinectBase.x    = cos(yawRadians) * joint.x 
                    + sin(yawRadians) * joint.z;
    kinectBase.y    = joint.y;
    kinectBase.z    = -sin(yawRadians) * joint.x 
                    + cos(yawRadians) * joint.z;
    
/// Convert from Kinect axis to robot axis
    Pos robotBase;

    robotBase.x = kinectBase.x;
    robotBase.y = kinectBase.z;
    robotBase.z = -kinectBase.y;

    robotBase2.x    = robotBase.y
                    + kinectin_paikka_x;
    robotBase2.y    = -robotBase.x
                    + kinectin_paikka_y;
    robotBase2.z    = robotBase.z
                    + kinectin_paikka_z;
#else
    // This calculations is using calculated transformation matrix

    // (0 | -sin(a) | cos(a)  | kinectin_etaisyys_kinect_basesta + kinectin_paikka_x - kinectin_varren_pituus*sin(a)
    // -1 | 0       | 0       | kinectin_paikka_y
    // 0  | -cos(a) | -sin(a) | kinectin_paikka_z - kinectin_varren_pituus*cos(a)
    // 0  | 0       | 0       | 1)
    robotBase2.x    = position.y * -sin(elevationRadians)
                    + position.z * cos(elevationRadians)
                    + kinectin_etaisyys_kinect_basesta + kinectin_paikka_x - (kinectin_varren_pituus * sin(elevationRadians));

    robotBase2.y    = -position.x
                    + kinectin_paikka_y;

    robotBase2.z    = position.y * -cos(elevationRadians)
                    + position.z * -sin(elevationRadians)
                    + kinectin_paikka_z - (kinectin_varren_pituus * cos(elevationRadians));

#endif
    return robotBase2;
}

double CoordinatorConverter::limitAngle(double angle)
{
    //Limit angle between 0-2Pi.
    return angle-floor(angle/(2*Pii))*2*Pii;
}
